#include "pi.h"

/*!
    \brief      PI Controller
    \param[in]  
    \param[out] 
    \retval
 */

void PI_Run(PIDREG3 *v)
{
    v->Err = v->Ref - v->Fdb;

    v->Up = _IQ15mpy(v->Kp, v->Err);
    v->Ui = v->Ui + _IQ15mpy(v->Ki, v->Up) + _IQ15mpy(v->Kc, v->SatErr);
    v->Ud = _IQ15mpy(v->Kd,(v->Up-v->Up1));

    v->OutPreSat = v->Up + v->Ui + v->Ud; 
    v->Out = (v->OutPreSat>v->OutMax) ? v->OutMax : v->OutPreSat;
    v->Out = (v->Out < v->OutMin) ? v->OutMin : v->Out;

    v->SatErr = v->Out - v->OutPreSat;
    v->Up1 = v->Up;
}

